
#include "Geometry.h"

using namespace ttg::math;

//----------------
//--- Triangle ---
//----------------

bool Triangle::isInnerPoint(const vec3f &point)
{
	vec3f a = (this->p2() - this->p1()).normalize();
    vec3f b = (this->p3() - this->p1()).normalize();
    vec3f c = (this->p2() - this->p3()).normalize();
    vec3f r1 = (point - this->p1()).normalize();
    if (dotProduct(a, b) > dotProduct(b, r1))
        return false; else
    {
        vec3f c = (this->p2() - this->p3()).normalize();
        vec3f r3 = (point - this->p3()).normalize();
        float dp = dotProduct(c, -b);
        if (dp > dotProduct(c, r3))
            return false; else
        if (dp > dotProduct(-b, r3))
            return false;
    }
    return true;
}

bool Triangle::areIntersected(const Triangle &t1, const Triangle &t2, vec3f &ipoint)
{
	Plane pl1(t1.p1(), t1.p2(), t1.p3());
	Plane pl2(t2.p1(), t2.p2(), t2.p3());
	if (!pl1.isCorrect() || !pl2.isCorrect())
		return false;
	float va1 = pl2.getValueAt(t1.p1()),
		  vb1 = pl2.getValueAt(t1.p2()),
		  vc1 = pl2.getValueAt(t1.p3());
	float va2 = pl1.getValueAt(t2.p1()),
		  vb2 = pl1.getValueAt(t2.p2()),
		  vc2 = pl1.getValueAt(t2.p3());
	int i1 = isSignEqual(va1, vb1)
			 ? (isSignEqual(va1, vc1)
					? -1
					: 2)
			 : (isSignEqual(va1, vc1)
					? 1
					: (isSignEqual(vc1, vb1)
						? 0
						: 1));
	int i2 = isSignEqual(va2, vb2)
			 ? (isSignEqual(va2, vc2)
					? -1
					: 2)
			 : (isSignEqual(va2, vc2)
					? 1
					: (isSignEqual(vc2, vb2)
						? 0
						: 1));
	if (i1 < 0 || i2 < 0)
		return false;
	Line il(pl1, pl2);
	if (!il.isCorrect())
		return false;
	vec3f ip[4];
	if (!Line::areIntersected(il, Line(t1[(i1 + 1)%3] - t1[i1], t1[i1]), ip[0]) ||
		!Line::areIntersected(il, Line(t1[(i1 + 2)%3] - t1[i1], t1[i1]), ip[1]) ||
		!Line::areIntersected(il, Line(t2[(i2 + 1)%3] - t2[i2], t2[i2]), ip[2]) ||
		!Line::areIntersected(il, Line(t2[(i2 + 2)%3] - t2[i2], t2[i2]), ip[3]))
		return false;
		//throw Error(Error::FATAL, "GGE");
	float ipp11 = dotProduct(ip[0], il.P()); 
	float ipp12 = dotProduct(ip[1], il.P());
	float ipp21 = dotProduct(ip[2], il.P());
	float ipp22 = dotProduct(ip[3], il.P());
	float t;
	if (ipp11 > ipp12)
	{
		t = ipp11;
		ipp11 = ipp12;
		ipp12 = t;
	}
	if (ipp21 > ipp22)
	{
		t = ipp21;
		ipp21 = ipp22;
		ipp22 = t;
	}
	if (ipp21 >= ipp12 || ipp22 <= ipp11)
		return false;
	ipoint = (ip[ipp11 > ipp21 ? 0 : 2] + ip[ipp12 > ipp22 ? 3 : 1]) / 2.0f;
	return true;
}

//----------------
//--- Plane ---
//----------------

Plane::Plane(float A, float B, float C, float D)
{
    this->coeff[0] = A;
    this->coeff[1] = B;
    this->coeff[2] = C;
    this->coeff[3] = D;
    nonzero = !isZero(coeff[1]) ? 1 : !isZero(coeff[2]) ? 2 :
#ifdef TTG_DEBUG
			!isZero(coeff[0]) ? 0 : -1;
#else
			0;
#endif
}

Plane::Plane(vec3f a, vec3f b, const vec3f &c)
{
    a -= c;
    b -= c;
    coeff[3]= -c.x * (coeff[0] = a.y * b.z - a.z * b.y) +
              -c.y * (coeff[1] = -(a.x * b.z - a.z * b.x)) +
              -c.z * (coeff[2] = a.x * b.y - a.y * b.x);
    nonzero = !isZero(B()) ? 1 : !isZero(C()) ? 2 :
#ifdef TTG_DEBUG
			!isZero(A()) ? 0 : -1;
#else
			0;
#endif
}

Plane::Plane(const vec3f &point, const vec3f &normal)
{
	coeff[0] = normal.x;
	coeff[1] = normal.y;
	coeff[2] = normal.z;
	coeff[3] = -(A() * point.x + B() * point.y + C() * point.z);
	nonzero = !isZero(B()) ? 1 : !isZero(C()) ? 2 :
#ifdef TTG_DEBUG
			!isZero(A()) ? 0 : -1;
#else
			0;
#endif
}

Plane::Plane(const Triangle &t)
{
	vec3f a = t.p1() - t.p3();
    vec3f b = t.p2() - t.p3();
    coeff[3]= -t.p3().x * (coeff[0] = a.y * b.z - a.z * b.y) +
              -t.p3().y * (coeff[1] = -(a.x * b.z - a.z * b.x)) +
              -t.p3().z * (coeff[2] = a.x * b.y - a.y * b.x);
    nonzero = !isZero(B()) ? 1 : !isZero(C()) ? 2 :
#ifdef TTG_DEBUG
			!isZero(A()) ? 0 : -1;
#else
			0;
#endif
}

const Plane Plane::operator -(const Plane &p) const
{
	return Plane(coeff[0] - p.coeff[0],
					coeff[1] - p.coeff[1],
					coeff[2] - p.coeff[2],
					coeff[3] - p.coeff[3]);
}

const Plane Plane::operator +(const Plane &p) const
{
	return Plane(coeff[0] + p.coeff[0],
					coeff[1] + p.coeff[1],
					coeff[2] + p.coeff[2],
					coeff[3] + p.coeff[3]);
}

const Plane &Plane::operator =(const Plane &p)
{
	coeff[0] = p.coeff[0];
	coeff[1] = p.coeff[1];
	coeff[2] = p.coeff[2];
	coeff[3] = p.coeff[3];
	return *this;
}

const Plane Plane::operator *(float t) const
{
	return Plane(coeff[0] * t,
					coeff[1] * t,
					coeff[2] * t,
					coeff[3] * t);
}

bool Plane::areIntersected(const Plane &p, const Line &l, float &t)
{
    double c = dotProduct(l.P(), p.getNormal());
    if (isZero(c))
        return false; //they are parallel;
    t = (float)(-(p.D() + dotProduct(l.Q(), p.getNormal())) / c);
	return true;
}

bool Plane::areIntersected(const Plane &p, const Line &l, vec3f &ipoint)
{
	float t;
	bool res = Plane::areIntersected(p, l, t);
	if (res)
		ipoint = l.getValueAt(t);
	return res;
}

//---------------
//--- Line ---
//---------------

Line::Line(const vec3f &P, const vec3f &Q)
{
    this->_P = P;
    this->_Q = Q;
    correct = P != vec3f();
}

Line::Line(const Plane &m, const Plane &n)
{
    correct = false;
	int ix = m.getIndexOfNonZeroCoeff();
    int iy, iz;
    int i1 = (ix+2)%3;
    int i2 = (ix+1)%3;
    float Ey, E1 = n[i1]*m[ix] - n[ix]*m[i1];
    float Ez, E2 = n[i2]*m[ix] - n[ix]*m[i2];
    iy = !isZero(E1) ? i1 : !isZero(E2) ? i2 : 0xFF;
    if (iy == 0xFF)
        return;  //they are parallel
    bool flag = (iy == i1);
    iz = flag ? i2 : i1;
    Ey = flag ? E1 : E2;
    Ez = flag ? E2 : E1;
    this->_P[iz] = 1.0f;
    this->_Q[iz] = 0.0f;
    this->_P[iy] = -Ez / Ey;
    this->_Q[iy] = (m.D() * n[ix] - m[ix] * n.D()) / Ey;
    this->_P[ix] = -(m[iz] + m[iy] * _P[iy]) / m[ix];
    this->_Q[ix] = -(m.D() + m[iy] * _Q[iy]) / m[ix];
    correct = true;
}

bool Line::areIntersected(const Line &l1, const Line &l2, vec3f &ipoint)
{
	if (!l1.isCorrect() || !l2.isCorrect())
		return false;
	vec3f n = crossProduct(l1.P(), l2.P());
	vec3f dQ = l1.Q() - l2.Q();
	if (n == vec3f()) // they are parallel
	{
		if (dQ == vec3f())  //they are identical
		{
			ipoint = l2.Q() + l2.P() / 2;
			return true;
		}
		else
			return false;
	}
	n = crossProduct(n, l1.P()).normalize();
	float p = dotProduct(n, l2.P());
	float q = dotProduct(n, dQ);
	ipoint = l2.P() * (q / p) + l2.Q();
	return true;
}
